Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

The Pick And Place Autonomous Demo

Overview

pick_and_place_demo.py is an autonomous example demo built on top of pick_and_place_manager.py, which contains Python functions that you can use to easily script any sort of demo where you want the robot to pick up well-separated objects (~3 cm apart) at or near specified locations on flat surfaces, and then to place/drop the objects somewhere inside specified rectangular regions in space. Look in pick_and_place_demo.py to see how to use these functions, as well as how to overload functions whose behavior you wish to modify (particularly for preempting, or for adding to the keyboard interface).

IMPORTANT: arm_navigation, on which pr2_interactive_manipulation and the pick_and_place_demo are currently based, depends on a dry version of geometric_shapes contained in that stack. This package is incompatible with the wet version used by MoveIt!. If you have ros-groovy-geometric-shapes installed, you must do

sudo apt-get remove ros-groovy-geometric-shapes

in order for arm_navigation and thus pr2_interactive_manipulation and pr2_pick_and_place_demo to work.

Running the demo implies the following:

• on the robot:

  • bring up the robot

  • launch Interactive Manipulation

  • position the robot in a good position relative to the table, with the torso up and the head looking at the objects

  • start the demo executive, which provides both continuous autonomous operation and step-by-step operation based on keyboard commmands

• on a separate computer with a screen:

  • run Interactive Manipulation desktop (rviz with special config file) to see what's going on

NOTE: To find out more about how to teleop the robot to do manipulation tasks using Interactive Manipulation, look at the pr2_interactive_manipulation package.

Starting the Demo

Make sure you have done a rosmake of pr2_pick_and_place_demos on the robot before you launch the robot, if you are using anything but pre-built debs. To install everything you need in debians in Groovy in Ubuntu, just do

sudo apt-get ros-groovy-pr2-interactive-manipulation

Until the next release, you'll also have to clone pr2_object_manipulation (branch groovy-devel) in order to get the Pick and Place Demo: https://github.com/ros-interactive-manipulation/pr2_object_manipulation

On the robot, run the following in separate terminals:

Real robot bring-up:

robot start

Sim robot bring-up:

roslaunch pr2_gazebo pr2_table_object.launch

Start the manipulation pipeline, in the context of Interactive Manipulation. In general, all you have to do is:

on real PR2s:

roslaunch pr2_interactive_manipulation pr2_interactive_manipulation_robot

on simulated PR2s:

export ROBOT=sim
roslaunch pr2_interactive_manipulation pr2_interactive_manipulation_robot.launch sim:=true reactive_grasping:=true

Start the demo itself:

rosrun pr2_pick_and_place_demos pick_and_place_demo.py

Position the robot so that the front edge of the base is flush with the front edge of the table. For most tables (all but really low coffee tables), the torso should be all the way up so the robot can reach the objects on it. If you're unsure what height is appropriate, hit the runstop and manually move the arms around to see if you can grasp objects on the table (from the top, not just from the side). Objects should be well-separated (>=3 cm between).

On a sim PR2, you can do this using interactive manipulation: Click a few times at the bottom of the PublishClickCamera window to make the robot look down at the table and object. Switch from ‘Move Camera’ mode in Rviz to ‘Interact’ mode, so you can use the interactive markers (from pr2_marker_control). Right-click on the bicep of each robot arm in turn, and select ‘Forced move to side’ to get the arms out of the way.

On a separate computer with a screen, run the following in separate terminals: for real PR2s:

rosrun pr2_dashboard pr2_dashboard

roslaunch pr2_interactive_manipulation_frontend pr2_interactive_manipulation_desktop.launch

for sim PR2s:

roslaunch pr2_interactive_manipulation_frontend pr2_interactive_manipulation_desktop.launch sim:=true

Useful additional topics to add to your rviz window to see what's going on in the grasp demo can be found here: Useful rviz topics for the grasp pipeline One additional topic of use for this demo specifically is the Marker topic /grasp_markers, which shows the possible spots for placing the object at various rotations.

Running the Demo

The demo executive provides both continuous and step-by-step operation. In the continuous mode, the robot will move objects from one side of the table to another. In step-by-step mode, you can use keyboard commands to perform individual actions.

Here is the root menu of the demo executive:

type:
'start' to start the autonomous demo
s to switch pick-up and put-down sides
hs to point the head at either side
r to control the right arm, l to control the left arm
d to detect objects, dc to detect and take a new static collision map, dca to detect, take a new collision map, and clear attached objects
p to pick up an object
w to place the object in the place rectangle, wo to place the object where it came from
h to point the head at the current place rectangle and draw it
s to switch pick-up and put-down sides
t to find the table
rm to reset the collision map, tm to take a new collision map
det to detach the object in the gripper
q to quit
press enter to continue

Continuous mode

Inside the terminal with pick_and_place_demo.py running, type 'start' and press enter. The robot will go into continuous mode, and keep running until it encounters an error that it does not know how to recover from.

The keyboard interface allows you to preempt the autonomous demo and go back to a useful menu of keyboard commands by typing 's' and then enter. It will go back after the last service call was completed, which may take awhile. You can also pause it by typing 'p' and enter.

Always have the run-stop nearby, so that you can hit it if and when the robot does something crazy.

The keyboard interface

The keyboard interface is a good way to get the robot out of stuck situations in a manner more graceful than shutting down motors/manually opening the gripper and putting the robot back in a default state. Particularly useful commands (and these are all printed on the screen for easy reference) include:

• 'w' to place an object the robot is holding back on the table (this should only be done if the arms are both off to the side; if they are not, move them there first. If the robot doesn't remember what the object is, perhaps because the program was just started up, it will give you the option of dropping it semi-blindly.)
• 'r' to control the right arm
• 'l' to control the left arm

Within each arm control menu are the following options:

c to close, o to open the gripper
u to go up, d to go down, rt to go right, lt to go left
ay to go away from the robot, td to go toward the robot
jm to go to arm-away-on-the-side angles using move_arm
jc to go to arm-away-on-the-side angles while trying to constrain the pose
j to go to arm-away-on-the-side angles open-loop using the joint trajectory action
e to exit the arm-control menu for this arm (r)
press enter to continue

Notes:

• 'jm' moves the arm back to the side position using the motion planner, in an attempt not to hit things, then falls back on moving open-loop if the motion planner fails.
• 'j' moves the arm back to the side position without avoiding collisions (but the trajectory is hard-coded so as to be OK in most cases, so it is useful for moving back in a hurry when you're pretty sure there's nothing in the way).
• up/down movement commands move 10 cm at a time, while left/right/toward/away movement commands move 2 cm at a time, without avoiding collisions.

A common useful sequence, if the robot gets stuck with its hand around an object on the table when everything crashes:

Once the demo executive is back up, if the right arm is the stuck one, you can type, in rapid succession, separated by 'enter's (the commands will queue up, with each executing after the last finishes):

• 'r' (right arm control menu)
• 'o' (open the gripper)
• 'u' (move the arm up and away from the object by 10 cm)
• 'jm' (move the arm back to the side while trying not to collide with anything)

Troubleshooting/Recovering from stuck situations

If at any point the robot appears to be stuck, look at the following to see what is wrong:

• rviz
• the dashboard robot status window
• the dashboard console
• the terminal with pick_and_place_demo.py
• the terminal with pr2_tabletop_manipulation.launch
• the terminal with robot.launch

Semi-common things that can go wrong, and the solutions to them

• Most common failure mode: two or more objects are getting clumped into one giant cluster (one green bounding box will encapsulate more than one object).

  • It is generally a good idea to track rviz and see if this happens
  • Keep in mind that even if objects start up far away from each other, they might end up too close as the robot is moving them around
  • If it happens, separate the objects on the table more. The robot will eventually re-detect and try again to grasp the nearest object.
  • If objects end up close to each other, you might want to surreptitiously nudge an object every now and then to create more breathing room
• The robot keeps trying to pick up thin air:
  • See the above case, it is the most likely cause
• The robot keeps trying and failing to grasp one object:
  • Remove the object. Explain that our algorithms are not perfect to any gawkers, and give the robot an easier toy to play with.
• Robot just waits and does nothing
  • Check the various terminals. It's most likely just spending a really long time doing a tricky bit of motion planning.
• The demo executive is telling me that a serious error occurred, and to press 'q' to go back to the keyboard interface:
  • Check the other windows to see why a serious error occurred. This often means that one or more crucial things have died, and may need manual restarting.
  • If it looks like whatever tripped the error has fixed itself, press 'q' and 'enter' to go back to the keyboard interface, use the keyboard interface to put down any objects the robot may be holding, then type 'start' to start up the demo again.
  • Otherwise, you probably want to restart pr2_tabletop_manipulation.launch and pick_and_place_demo.py.
• The gripper keeps not closing all the way around objects/complaining that grasps failed because the gripper opening was unacceptable.

  • The gripper calibration is probably wrong. Close both grippers and do rostopic echo /joint_states/position[23] and rostopic echo /joint_states/position[37]. Both numbers should be very near-zero (no more than about .002 away). If this is not the case, disable all breakers in the dashboard, re-enable them, and re-launch the robot (this forces the robot to recalibrate its joints). Make sure there is nothing in either gripper during calibration.

• Things are just frozen:
  • Try restarting just pr2_interactive_manipulation_robot.launch and pick_and_place_demo.py. If that fails, re-launch the robot.
  • If the robot is still screwed up, reboot the robot entirely.